clear;
clc;
close all;


%Torquw force wrench starts here
endline = 1001;
data2_a = csvread('tmp/result2_1000_a.txt',1,0,[1 0 endline-1 21]);
data2_b = csvread('tmp/result2_1000_b.txt',1,0,[1 0 endline-1 21]);
data2_c = csvread('tmp/result2_1000_c.txt',1,0,[1 0 endline-1 21]);
data2_d = csvread('tmp/result2_1000_d.txt',1,0,[1 0 endline-1 21]);
data2_e = csvread('tmp/result2_1000_e.txt',1,0,[1 0 endline-1 21]);
data2_f = csvread('tmp/result2_1000_f.txt',1,0,[1 0 endline-1 21]);
data2_g = csvread('tmp/result2_1000_g.txt',1,0,[1 0 endline-1 21]);
data2_h = csvread('tmp/result2_1000_h.txt',1,0,[1 0 endline-1 21]);
data2_i = csvread('tmp/result2_1000_i.txt',1,0,[1 0 endline-1 21]);
data2_j = csvread('tmp/result2_1000_j.txt',1,0,[1 0 endline-1 21]);
data2 = [data2_a;data2_b; data2_c;data2_d;data2_e;data2_f;data2_g;data2_h;data2_i;data2_j];

succ2Ind = find(data2(:,3)==1 & data2(:,4)==1);

res2 = data2(:,3);
res2 = res2 & data2(:,4);
for i=6:3:22
    res2 = res2 & data2(:,i) & data2(:,i+1);
end;


x2=data2(succ2Ind,2);
y2=res2(succ2Ind);

h1=figure(1);
plot(x2,y2,'.');
axis([min(x2)*0.99 max(x2)*1.01 -0.2 1.2])
grid on;
xlabel('Minimum Wrench [N]','FontWeight','bold');
ylabel('Succeeded','FontWeight','bold');
title('TorqueForce Wrench space','FontWeight','bold','FontSize',14);

failures2 = size(find(y2 == 0),1)
success2 = size(find(y2 == 1),1)

k=0
successes2 = size(find((data2(succ2Ind,3) & data2(succ2Ind,4))==1),1);
k=k+1; %fail2(1) = size(find((data2(succInd,3) & data(succ2Ind,4))==1),1);
k=k+1; fail2(k) = size(find((data2(succ2Ind,6) & data2(succ2Ind,7))==1),1);
k=k+1; fail2(k) = size(find((data2(succ2Ind,9) & data2(succ2Ind,10))==1),1);
k=k+1; fail2(k) = size(find((data2(succ2Ind,12) & data2(succ2Ind,13))==1),1);
k=k+1; fail2(k) = size(find((data2(succ2Ind,15) & data2(succ2Ind,16))==1),1);
k=k+1; fail2(k) = size(find((data2(succ2Ind,18) & data2(succ2Ind,19))==1),1);
k=k+1; fail2(k) = size(find((data2(succ2Ind,21) & data2(succ2Ind,22))==1),1);

h2=figure(2);

plot(fail2./successes2,'s','MarkerEdgeColor','k','MarkerFaceColor','g','MarkerSize',6);
set(gca,'XTickLabel',{'Xp','Xn','Yp','Yn','Zp','Zn'})
ylabel('Success Rate [%]','FontWeight','bold');
xlabel('Direction','FontWeight','bold');
axis([1.1 k+0.9 0.83 1.01]);
grid on;
title('Torque Force wrench - Directional successrate','FontSize',14,'FontWeight','bold');

h3=figure(3);


k=0;
for i=1:7
k = k+1; ind = find(x2 >= (max(x2)-min(x2))/7*i+min(x2) & x2 < (max(x2)-min(x2))/7*(i+1)+min(x2));
succ2(k) = size(find(y2(ind) == 1),1) / size(ind,1); size(ind,1)
end;
plot(succ2*100,'s','MarkerEdgeColor','k','MarkerFaceColor','g','MarkerSize',6);
ylabel('Success Rate [%]','FontWeight','bold');
xlabel('Minimum Wrench [N]','FontWeight','bold');
grid on;
title('Torque force wrench - Successrate','FontWeight','bold','FontSize',14)
%axis([0 k+1 -5 105]);
set(gca,'XTick',1:size(succ2,2))
%set(gca,'XTickLabel',{'0-0.50','0.50-0.75','0.75-1.0','1.0-1.25','1.25-1.50','1.50-'})

%save figure to files
saveas(h1,'../report/figures/torfor1','png') 
saveas(h2,'../report/figures/torfor2','png') 
saveas(h3,'../report/figures/torfor3','png') 

%cmccp wrench starts here
data_a = csvread('Results/result_1000_cmcpp_a.txt',1,0,[1 0 endline-1 21]);
data_b = csvread('Results/result_1000_cmcpp_b.txt',1,0,[1 0 endline-1 21]);
data_c = csvread('Results/result_1000_cmcpp_c.txt',1,0,[1 0 endline-1 21]);
data_d = csvread('Results/result_1000_cmcpp_d.txt',1,0,[1 0 endline-1 21]);
data_e = csvread('Results/result_1000_cmcpp_e.txt',1,0,[1 0 endline-1 21]);
data_f = csvread('Results/result_1000_cmcpp_f.txt',1,0,[1 0 endline-1 21]);
data_g = csvread('Results/result_1000_cmcpp_g.txt',1,0,[1 0 endline-1 21]);
data_h = csvread('Results/result_1000_cmcpp_h.txt',1,0,[1 0 endline-1 21]);
data_i = csvread('Results/result_1000_cmcpp_i.txt',1,0,[1 0 endline-1 21]);
data_j = csvread('Results/result_1000_cmcpp_j.txt',1,0,[1 0 endline-1 21]);
data = [data_a;data_b; data_c;data_d;data_e;data_f;data_g;data_h;data_i;data_j];

succInd = find(data(:,3)==1 & data(:,4)==1);

res = data(:,3);
res = res & data(:,4);
for i=6:3:22
    res = res & data(:,i) & data(:,i+1);
end;

% X
%res = res & data(:,6) & data(:,7);
%res = res & data(:,9) & data(:,10);

% Y
%res = res & data(:,12) & data(:,13);
%res = res & data(:,15) & data(:,16);

% Z
%res = res & data(:,18) & data(:,19);
%res = res & data(:,21) & data(:,22);

x=data(succInd,2);
y=res(succInd);

h4=figure(4);
plot(x,y,'.');
axis([min(x)*0.99 max(x)*1.01 -0.2 1.2])
grid on;
xlabel('Wrench [1/m]','FontWeight','bold');
ylabel('Succeeded','FontWeight','bold');
title('CMCCP','FontWeight','bold','FontSize',14);

failures = size(find(y == 0),1)
success = size(find(y == 1),1)

k=0
successes = size(find((data(succInd,3) & data(succInd,4))==1),1);
k=k+1; %fail(1) = size(find((data(succInd,3) & data(succInd,4))==1),1);
k=k+1; fail(k) = size(find((data(succInd,6) & data(succInd,7))==1),1);
k=k+1; fail(k) = size(find((data(succInd,9) & data(succInd,10))==1),1);
k=k+1; fail(k) = size(find((data(succInd,12) & data(succInd,13))==1),1);
k=k+1; fail(k) = size(find((data(succInd,15) & data(succInd,16))==1),1);
k=k+1; fail(k) = size(find((data(succInd,18) & data(succInd,19))==1),1);
k=k+1; fail(k) = size(find((data(succInd,21) & data(succInd,22))==1),1);

h5=figure(5);

plot(fail./successes,'s','MarkerEdgeColor','k','MarkerFaceColor','y','MarkerSize',6);
set(gca,'XTickLabel',{'Xp','Xn','Yp','Yn','Zp','Zn'})
ylabel('Success Rate [%]','FontWeight','bold');
xlabel('Direction','FontWeight','bold');
axis([1.1 k+0.9 0.83 1.01]);
grid on;
title('Directional successrate','FontWeight','bold','FontSize',14);

h6=figure(6);
% k=0;
% step = 0.5;
% for i=step:step:max(x)
%    k = k+1;
%    ind = find(x >= i-step & x < i);
%    if size(ind,1) > 0
%        succ(k) = size(find(y(ind) == 1),1) / size(ind,1);
%    else
%        succ(k) = -0.1;
%    end;
% end;
% plot(succ,'x');
% ylabel('Success rate');

% k=0;
% k = k+1; ind = find(x >= 0.0 & x < 1.5); succ(k) = size(find(y(ind) == 1),1) / size(ind,1); size(ind,1)
% k = k+1; ind = find(x >= 1.5 & x < 2.0); succ(k) = size(find(y(ind) == 1),1) / size(ind,1); size(ind,1)
% k = k+1; ind = find(x >= 2.0 & x < 2.5); succ(k) = size(find(y(ind) == 1),1) / size(ind,1); size(ind,1)
% k = k+1; ind = find(x >= 2.5 & x < 3.0); succ(k) = size(find(y(ind) == 1),1) / size(ind,1); size(ind,1)
% k = k+1; ind = find(x >= 3.0 & x < 3.5); succ(k) = size(find(y(ind) == 1),1) / size(ind,1); size(ind,1)
% k = k+1; ind = find(x >= 3.5 & x < 4.0); succ(k) = size(find(y(ind) == 1),1) / size(ind,1); size(ind,1)
% k = k+1; ind = find(x >= 4.0 & x < 4.5); succ(k) = size(find(y(ind) == 1),1) / size(ind,1); size(ind,1)
% k = k+1; ind = find(x >= 4.5 & x < 5.0); succ(k) = size(find(y(ind) == 1),1) / size(ind,1); size(ind,1)
% k = k+1; ind = find(x >= 5.0 & x < 5.5); succ(k) = size(find(y(ind) == 1),1) / size(ind,1); size(ind,1)
% k = k+1; ind = find(x >= 5.5 & x < 6.0); succ(k) = size(find(y(ind) == 1),1) / size(ind,1); size(ind,1)
% k = k+1; ind = find(x >= 6.0 & x < 6.5); succ(k) = size(find(y(ind) == 1),1) / size(ind,1); size(ind,1)
% k = k+1; ind = find(x >= 6.5 & x < 7.0); succ(k) = size(find(y(ind) == 1),1) / size(ind,1); size(ind,1)
% k = k+1; ind = find(x >= 7.0); succ(k) = size(find(y(ind) == 1),1) / size(ind,1); size(ind,1)
% plot(succ,'x');
% ylabel('Success rate');
% axis([0 k+1 -0.05 1.05]);
% set(gca,'XTick',1:size(succ,2))
% set(gca,'XTickLabel',{'0-1.5','1.5-2.0','2.0-2.5','2.5-3.0','3.0-3.5','3.5-4.0','4.0-4.5','4.5-5.0','5.0-5.5','5.5-6.0','6.0-6.5','6.5-7.0','7.0-'})

% k=0;
% mass = 0.372;
% g = 9.82;
% k = k+1; ind = find(x >= 0.0*g*mass & x < 0.5*g*mass); succ(k) = size(find(y(ind) == 1),1) / size(ind,1); size(ind,1)
% k = k+1; ind = find(x >= 0.5*g*mass & x < 0.75*g*mass); succ(k) = size(find(y(ind) == 1),1) / size(ind,1); size(ind,1)
% k = k+1; ind = find(x >= 0.75*g*mass & x < 1.0*g*mass); succ(k) = size(find(y(ind) == 1),1) / size(ind,1); size(ind,1)
% k = k+1; ind = find(x >= 1.0*g*mass & x < 1.25*g*mass); succ(k) = size(find(y(ind) == 1),1) / size(ind,1); size(ind,1)
% k = k+1; ind = find(x >= 1.25*g*mass & x < 1.5*g*mass); succ(k) = size(find(y(ind) == 1),1) / size(ind,1); size(ind,1)
% k = k+1; ind = find(x >= 1.5*g*mass); succ(k) = size(find(y(ind) == 1),1) / size(ind,1); size(ind,1)
k=0;
for i=1:7
k = k+1; ind = find(x >= (max(x)-min(x))/7*i+min(x) & x < (max(x)-min(x))/7*(i+1)+min(x));
succ(k) = size(find(y(ind) == 1),1) / size(ind,1); size(ind,1)
end;
plot(succ*100,'s','MarkerEdgeColor','k','MarkerFaceColor','y','MarkerSize',6);
ylabel('Success Rate [%]','FontWeight','bold');
xlabel('Minimum Wrench [1/m]','FontWeight','bold');
grid on;
title('Successrate vs. wrench measure','FontWeight','bold','FontSize',14)
%axis([0 k+1 -5 105]);
set(gca,'XTick',1:size(succ,2))
%set(gca,'XTickLabel',{'0-0.50','0.50-0.75','0.75-1.0','1.0-1.25','1.25-1.50','1.50-'})


%save figure to files
saveas(h4,'../report/figures/cmcpp1','png') 
saveas(h5,'../report/figures/cmcpp2','png') 
saveas(h6,'../report/figures/cmcpp3','png') 
